//
// Created by Pulsar on 2020/5/16.
//
#include <iostream>
#include <rc_system/config.h>
#include <rc_system/context.h>

#include <signal.h>
#include <rc_log/slog.hpp>
#include <rc_task/rcMoveTask.h>
#include <rc_task/rcTaskVariable.h>

#include <rc_move/rcmove.h>

volatile sig_atomic_t flag = 1;

void sig_handler(int signum) {
    if (signum == SIGINT) {
        std::cout << "Exiting..." << std::endl;
        flag = 0;
    }
}

int main(int argc, char **argv) {
    signal(SIGINT, sig_handler);
    std::shared_ptr<rccore::common::Config> pconfig = std::make_shared<rccore::common::Config>("config.yml");
    if (pconfig->Load())
        pconfig->Load();
    std::shared_ptr<rccore::common::Context> pcontext = std::make_shared<rccore::common::Context>(pconfig);
    pcontext->Init();
    rccore::task::MoveTask move_task(pcontext);
    move_task.Run();
//    rccore::move::RobotCarMove robot(pcontext);
//    robot.init();
    WHEEL_DATA wheelData;
    pcontext->pmessage_server->pmoveMessage->push_message(wheelData);
    bool dir = true;
    while (flag) {
        if (dir) {
            wheelData.wheel_1_v_m_s += 0.001;
            wheelData.wheel_2_v_m_s += 0.001;
            wheelData.wheel_3_v_m_s += 0.001;
        } else {
            wheelData.wheel_1_v_m_s -= 0.001;
            wheelData.wheel_2_v_m_s -= 0.001;
            wheelData.wheel_3_v_m_s -= 0.001;
        }
        if (wheelData.wheel_1_v_m_s > 0.4 or wheelData.wheel_1_v_m_s < 0.001) {
            dir = !dir;
        }
//        robot.excute(wheelData);
        slog::err << "wheel_1_v_m_s:" << wheelData.wheel_1_v_m_s
                  << "wheel_2_v_m_s " << wheelData.wheel_2_v_m_s
                  << "wheel_3_v_m_s " << wheelData.wheel_3_v_m_s
                  << slog::endl;
        usleep(100000);
//        std::this_thread::sleep_for(std::chrono::microseconds(1000));
    }
    move_task.Stop();
    return 1;
}